/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2024, USTC
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of USTC nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Xinsheng Tang */

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

int main(int argc, char **argv)
{
  char c = 'x';
  ros::init(argc, argv, "moveit_tutorial");
  ros::NodeHandle node_handle;

  // 启动ROS的异步线程，用于接收机器人状态信息
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // 初始化
  // ^^^^^
  // MoveIt使用"planning group"对机器人的(多个)关节进行操作，并将它们存储在一个名为"JointModelGroup"的对象中。
  // 在UR系列的机械臂中，官方已经配置好了一个名为"manipulator"的planning group
  static const std::string PLANNING_GROUP = "manipulator";

  // "MoveGroupInterface"类用来对机器人进行诸如目标位姿(关节角)设定、轨迹规划、轨迹执行等操作
  // 通过指定要控制和规划的planning group的名称进行初始化。
  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);

  const moveit::core::JointModelGroup *joint_model_group =
      move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // "PlanningSceneInterface"类用来在机器人的虚拟交互环境（planning scene）中进行
  // 诸如添加、删除障碍物，将物体附着到机器人上等操作
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // 安全起见，我们将机器人的速度和加速度的最大值降低为实际最大值的5%
  move_group_interface.setMaxVelocityScalingFactor(0.05);
  move_group_interface.setMaxAccelerationScalingFactor(0.05);

  // 规划到目标关节角(关节空间)
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // 让我们设置一个关节空间目标，并向其移动。这将替换我们之前设置的位姿目标。
  //
  // 首先，创建一个指向当前机器人状态(RobotState)的指针
  // RobotState 包含了机器人当前的关节角度，角速度和角加速度信息
  moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState();

  // 接下来将关节角度复制到joint_group_positions中
  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  // 设置关节角度，UR系列的机械臂一共有6个关节，具体对应关系请自行尝试
  joint_group_positions[0] = -1.5;
  joint_group_positions[1] = -1.5;
  joint_group_positions[2] = -2.0;
  joint_group_positions[3] = -1.0;
  joint_group_positions[4] = 1.5;
  joint_group_positions[5] = 0;

  // 设置关节角目标
  move_group_interface.setJointValueTarget(joint_group_positions);

  bool success;
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  while (true)
  {
    success = (move_group_interface.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("MoveIt tutorial", "plan 1 (joint space) %s", success ? "SUCCEED, enter 'Y' to execute trajectory, otherwise replan" : "FAILED, enter any key to replan");
    std::cin >> c;

    // 执行轨迹，此时机械臂会运动，小心安全！
    if (c == 'Y' && success)
    {
      if (move_group_interface.execute(my_plan) == moveit::core::MoveItErrorCode::SUCCESS)
      {
        break;
      }
      else
      {
        ROS_INFO_NAMED("MoveIt tutorial", "execute failed, please check teaching panel. enter any key to replan");
        std::cin >> c;
      }
    }
  }

  // 规划到目标位姿(笛卡尔空间)
  // ^^^^^^^^^^^^
  // 设定目标位姿target_pose1（包括位置和姿态）
  geometry_msgs::Pose target_pose1;
  // 设置目标位置
  target_pose1.position.x = 0.15;
  target_pose1.position.y = 0.45;
  target_pose1.position.z = 0.2;
  // 设置目标姿态，采用四元数表示姿态
  target_pose1.orientation.x = 1.0;
  target_pose1.orientation.y = 0.0;
  target_pose1.orientation.z = 0.0;
  target_pose1.orientation.w = 0.0;
  // 设置目标位姿
  move_group_interface.setPoseTarget(target_pose1);

  // 规划到目标位姿
  while (true)
  {
    success = (move_group_interface.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("MoveIt tutorial", "plan 2 (cartesian space) %s", success ? "SUCCEED, enter 'Y' to execute trajectory, otherwise replan" : "FAILED, enter any key to replan");
    std::cin >> c;

    // 执行轨迹，此时机械臂会运动，小心安全！
    if (c == 'Y' && success)
    {
      if (move_group_interface.execute(my_plan) == moveit::core::MoveItErrorCode::SUCCESS)
      {
        break;
      }
      else
      {
        ROS_INFO_NAMED("MoveIt tutorial", "execute failed, please check teaching panel. enter any key to replan");
        std::cin >> c;
      }
    }
  }

  // 添加障碍物
  // ^^^^^^^^^
  moveit_msgs::CollisionObject collision_object;
  // 设置障碍物的参考坐标系(frame_id)
  collision_object.header.frame_id = move_group_interface.getPlanningFrame();

  // 设置障碍物的id
  collision_object.id = "box1";

  // 定义障碍物为一个盒子，并设置它的尺寸
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[primitive.BOX_X] = 1.5;
  primitive.dimensions[primitive.BOX_Y] = 0.1;
  primitive.dimensions[primitive.BOX_Z] = 0.4;

  // 定义盒子所放置的位置 (相对之前所设置的参考坐标系，即frame_id)
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.12;
  box_pose.position.y = 0.58;
  box_pose.position.z = 0.08;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  // 添加操作
  collision_object.operation = collision_object.ADD;

  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(collision_object);

  // 向环境中添加障碍物
  ROS_INFO_NAMED("MoveIt tutorial", "add object into the planning scene");
  planning_scene_interface.addCollisionObjects(collision_objects);

  // 避障规划
  // ^^^^^^^
  // 定义目标位姿
  geometry_msgs::Pose target_pose2;
  target_pose2.orientation.x = 1.0;
  target_pose2.orientation.y = 0.0;
  target_pose2.orientation.z = 0.0;
  target_pose2.orientation.w = 0.0;
  target_pose2.position.x = 0.15;
  target_pose2.position.y = 0.7;
  target_pose2.position.z = 0.23;
  move_group_interface.setPoseTarget(target_pose2);

  while (true)
  {
    success = (move_group_interface.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("MoveIt tutorial", "plan 3 (collision free) %s", success ? "SUCCEED, enter 'Y' to execute trajectory, otherwise replan" : "FAILED, enter any key to replan");
    std::cin >> c;

    // 执行轨迹，此时机械臂会运动，小心安全！
    if (c == 'Y' && success)
    {
      if (move_group_interface.execute(my_plan) == moveit::core::MoveItErrorCode::SUCCESS)
      {
        break;
      }
      else
      {
        ROS_INFO_NAMED("MoveIt tutorial", "execute failed, please check teaching panel. enter any key to replan");
        std::cin >> c;
      }
    }
  }

  // 将物体附着在机器人上
  // ^^^^^^^^^^^^^^^^^
  //
  // 我们可以将物体附着到机器人上，以便它随机器人一起移动。
  // 这模拟了拾取对象的过程，以便对其进行操作。
  // 规划在保证机器人本体和环境物体不发生碰撞的同时，还会避免附着物体和环境发生碰撞
  moveit_msgs::CollisionObject object_to_attach;
  object_to_attach.id = "cylinder1";

  // 定义一个圆柱
  shape_msgs::SolidPrimitive cylinder_primitive;
  cylinder_primitive.type = primitive.CYLINDER;
  cylinder_primitive.dimensions.resize(2);
  cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.1;
  cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;

  // 设定圆柱的参考坐标系和位姿
  object_to_attach.header.frame_id = move_group_interface.getEndEffectorLink();
  geometry_msgs::Pose grab_pose;
  grab_pose.orientation.w = 1.0;
  grab_pose.position.z = 0.06;

  // 首先，将圆柱添加到环境中
  object_to_attach.primitives.push_back(cylinder_primitive);
  object_to_attach.primitive_poses.push_back(grab_pose);
  object_to_attach.operation = object_to_attach.ADD;
  planning_scene_interface.applyCollisionObject(object_to_attach);

  // 将圆柱附着在机器人的末端
  move_group_interface.attachObject(object_to_attach.id, move_group_interface.getEndEffectorLink());
  ROS_INFO_NAMED("MoveIt tutorial", "attach object to the robot");

  move_group_interface.setPoseTarget(target_pose1);
  // 轨迹规划
  while (true)
  {
    success = (move_group_interface.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

    ROS_INFO_NAMED("MoveIt tutorial", "plan 4 (collision free with object attached) %s", success ? "SUCCEED, enter 'Y' to execute trajectory, otherwise replan" : "FAILED, enter any key to replan");
    std::cin >> c;

    // 执行轨迹，此时机械臂会运动，小心安全！
    if (c == 'Y' && success)
    {
      if (move_group_interface.execute(my_plan) == moveit::core::MoveItErrorCode::SUCCESS)
      {
        break;
      }
      else
      {
        ROS_INFO_NAMED("MoveIt tutorial", "execute failed, please check teaching panel. enter any key to replan");
        std::cin >> c;
      }
    }
  }

  // 解除附着并移除物体
  // ^^^^^^^^^^^^^^^^
  //
  // 将圆柱从机器人的末端解除附着
  ROS_INFO_NAMED("MoveIt tutorial", "detach the object");
  move_group_interface.detachObject(object_to_attach.id);

  ros::Duration(3).sleep();

  // 将圆柱和盒子从环境中删除
  ROS_INFO_NAMED("MoveIt tutorial", "remove all objects");
  std::vector<std::string> object_ids;
  object_ids.push_back(collision_object.id);
  object_ids.push_back(object_to_attach.id);
  planning_scene_interface.removeCollisionObjects(object_ids);

  ROS_INFO_NAMED("MoveIt tutorial", "enter any key to finish tutorial");
  std::cin >> c;

  ros::shutdown();
  return 0;
}